
public class RoboMagellan {

	/**
	 * @param args
	 */
	public static void main(String args[]) {

		Killer kill = new Killer();
		new Thread(kill).start();
		
		int distanceFinal = 0;
		double distance = 0.0, heading = 0.0;
		
		String start = 	  "$GPGGA,170834,4540.0093,N,11102.80130,W,1,05,1.5,280.2,M,-34.0,M,,,*75";
		String cone1 =    "$GPGGA,123519,4540.0243,N,11102.80790,W,1,08,0.9,545.4,M,-46.9,M,,,*47";
		String cone2 =    "$GPGGA,123519,4540.0397,N,11102.79210,W,1,08,0.9,545.4,M,-46.9,M,,,*47";
		String cone3 =    "$GPGGA,123519,4540.0404,N,11102.80560,W,1,08,0.9,545.4,M,-46.9,M,,,*47";

		//US, 
		Sensors sensors = new Sensors();	
		//Movement
		Movement ramrod = new Movement();
		ramrod.init();
		LCD lcd = new LCD();
		Navigation nav = new Navigation(ramrod, sensors, lcd);
		nav.findCone();
		//nav.findCone();
		
//		GPSLocation dest, curr;
//		GPS testSat = new GPS(lcd);
//		String coords;
//		dest = new GPSLocation(cone1);
//		boolean tester;	
//		lcd.printTop("-----GPS-----");
//		lcd.printBtm("Acquiring Fix");	
//		////////////////////////////////////////////////////////////
//		//////////////////      GPS      ///////////////////////////
//		////////////////////////////////////////////////////////////
//		try{	
//			while(true){			
//				if(kill.checkKillStatus()){
//					ramrod.Stop();	
//					lcd.printTop("kill");
//					break;
//				} 
//				else {
//					//get gps, goto location
//					coords = testSat.updateLocation();
//					if(!coords.equals("")){
//						curr = new GPSLocation(coords);
//						heading = curr.bearingTo(dest);
//						distance = ((3.28 * curr.distanceTo(dest)) + 13);						
//						distanceFinal = (int) distance;
//						lcd.printTop("dist: " + distanceFinal);
//						lcd.printBtm("head: " + heading);
//						Thread.sleep(2000);						
//						break;
//					}else{
//						lcd.printTop("-----GPS-----");
//						lcd.printBtm("Acquiring Fix");
//					}
//				}
//			}
//			}catch(Exception e){
//				e.printStackTrace();
//			}
//		
//		////////////////////////////////////////////////////////////
//		////////////////// 1ST CONE GOTO STUFF       ///////////////
//		////////////////////////////////////////////////////////////                     
//		try{	
//		while(true){			
//			//if killswitch set
//			if(kill.checkKillStatus()){
//				ramrod.Stop();	
//				lcd.printTop("kill");
//				break;
//			} else {
//				tester = nav.checkNavStatus();
//				if(tester){
//					nav.goToLoc(heading, distanceFinal);
//				} else {
//					ramrod.Stop();
//					break;
//				}
//			}
//		}
//		lcd.printTop("Cone 1!");
//		lcd.printBtm("");
//		Thread.sleep(10000);
//		}catch(Exception e){
//			e.printStackTrace();
//		}finally {
//			lcd.printTop("");
//			lcd.printBtm("");
//		}
//
//		////////////////////////////////////////////////////////////
//		//////////////////      GPS      ///////////////////////////
//		////////////////////////////////////////////////////////////
//			dest = new GPSLocation(cone2);
//			try{
//				distance = 0.0;
//				heading = 0.0;
//				distanceFinal = 0;
//				while(true){			
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} 
//					else {
//						//get gps, goto location
//						coords = testSat.updateLocation();
//						if(!coords.equals("")){
//							curr = new GPSLocation(coords);
//							heading = curr.bearingTo(dest) + 21;
//							distance = ((3.28 * curr.distanceTo(dest)) - 5);						
//							distanceFinal = (int) distance;
//							lcd.printTop("dist: " + distanceFinal);
//							lcd.printBtm("head: " + heading);
//							Thread.sleep(2000);						
//							break;
//						}else{
//							lcd.printTop("-----GPS-----");
//							lcd.printBtm("Acquiring Fix");
//						}
//					}
//				}
//				}catch(Exception e){
//					e.printStackTrace();
//				}
//			
////			////////////////////////////////////////////////////////////
////			////////////////// 2ND CONE GOTO STUFF       ///////////////
////			////////////////////////////////////////////////////////////
//			try{	
//				Thread.sleep(3000);
//				sensors.resetOdo();
//				lcd.printBtm("odo2: "+sensors.getOdoCount());
//				Thread.sleep(2000);
//				nav.setNavStatus(true);
//				while(true){			
//					//if killswitch set
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} else {
//						tester = nav.checkNavStatus();
//						if(tester){
//							nav.goToLoc(heading, distanceFinal);
//						} else {
//							ramrod.Stop();
//							break;
//						}
//					}
//				}
//				lcd.printTop("Cone 2!");
//				lcd.printBtm("");
//				Thread.sleep(10000);
//			}catch(Exception e){
//				e.printStackTrace();
//			} finally {
//				
//				lcd.printTop("");
//				lcd.printBtm("");
//			}
//			
//			////////////////////////////////////////////////////////////
//			//////////////////      GPS      ///////////////////////////
//			////////////////////////////////////////////////////////////
//			dest = new GPSLocation(cone3);
//			try{	
//				distance = 0.0;
//				heading = 0.0;
//				distanceFinal = 0;
//				while(true){			
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} 
//					else {
//						//get gps, goto location
//						coords = testSat.updateLocation();
//						if(!coords.equals("")){
//							curr = new GPSLocation(coords);
//							heading = curr.bearingTo(dest) + 70;
//							distance = ((3.28 * curr.distanceTo(dest)) - 11);						
//							distanceFinal = (int) distance;
//							lcd.printTop("dist: " + distanceFinal);
//							lcd.printBtm("head: " + heading);
//							Thread.sleep(2000);						
//							break;
//						}else{
//							lcd.printTop("-----GPS-----");
//							lcd.printBtm("Acquiring Fix");
//						}
//					}
//				}
//				}catch(Exception e){
//					e.printStackTrace();
//				}
//			
////			////////////////////////////////////////////////////////////
////			////////////////// 3RD CONE GOTO STUFF       ///////////////
////			////////////////////////////////////////////////////////////
//			try{	
//				Thread.sleep(3000);
//				sensors.resetOdo();
//				lcd.printBtm("odo2: "+sensors.getOdoCount());
//				Thread.sleep(2000);
//				nav.setNavStatus(true);
//				while(true){			
//					//if killswitch set
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} else {
//						tester = nav.checkNavStatus();
//						if(tester){
//							nav.goToLoc(heading, distanceFinal);
//						} else {
//							ramrod.Stop();
//							break;
//						}
//					}
//				}
//				lcd.printTop("Cone 3!");
//				lcd.printBtm("");
//				Thread.sleep(10000);
//			}catch(Exception e){
//				e.printStackTrace();
//			} finally {	
//				lcd.printTop("");
//				lcd.printBtm("");
//			}
//			
//			////////////////////////////////////////////////////////////
//			//////////////////      GPS      ///////////////////////////
//			////////////////////////////////////////////////////////////
//			dest = new GPSLocation(start);
//			try{	
//				distance = 0.0;
//				heading = 0.0;
//				distanceFinal = 0;
//				while(true){			
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} 
//					else {
//						//get gps, goto location
//						coords = testSat.updateLocation();
//						if(!coords.equals("")){
//							curr = new GPSLocation(coords);
//							heading = curr.bearingTo(dest) + 65;
//							distance = ((3.28 * curr.distanceTo(dest)) + 45);						
//							distanceFinal = (int) distance;
//							lcd.printTop("dist: " + distanceFinal);
//							lcd.printBtm("head: " + heading);
//							Thread.sleep(2000);						
//							break;
//						}else{
//							lcd.printTop("-----GPS-----");
//							lcd.printBtm("Acquiring Fix");
//						}
//					}
//				}
//				}catch(Exception e){
//					e.printStackTrace();
//				}
//			
////			////////////////////////////////////////////////////////////
////			////////////////// START CONE GOTO STUFF       ///////////////
////			////////////////////////////////////////////////////////////
//			try{	
//				Thread.sleep(3000);
//				sensors.resetOdo();
//				lcd.printBtm("odo2: "+sensors.getOdoCount());
//				Thread.sleep(2000);
//				nav.setNavStatus(true);
//				while(true){			
//					//if killswitch set
//					if(kill.checkKillStatus()){
//						ramrod.Stop();	
//						lcd.printTop("kill");
//						break;
//					} else {
//						tester = nav.checkNavStatus();
//						if(tester){
//							nav.goToLoc(heading, distanceFinal);
//						} else {
//							ramrod.Stop();
//							break;
//						}
//					}
//				}
//			}catch(Exception e){
//				e.printStackTrace();
//			} finally {
//				lcd.printTop("**RAMRod WINS!**");
//				lcd.printBtm("****************");
//			}
	}
}
